/*
 * featureDetectionOnCamera.cpp
 *
 *  Created on: 09/04/2012
 *      Author: filipe
 */

#include <stdio.h>
#include <stdlib.h>
#include <vector>
#include "opencv/cv.h"
#include "opencv/cvaux.hpp"
#include "opencv/highgui.h"

using namespace std;
using namespace cv;

CvSeq* detectFeaturesWithSurf (IplImage* imagem)
{
	IplImage *gray = cvCreateImage(cvSize(imagem->width, imagem->height), imagem->depth, 1);
	cvCvtColor(imagem, gray, CV_BGR2GRAY);

	CvSeq *keypoints = NULL;
	CvSeq *descriptors = NULL;
	CvMemStorage *storage = cvCreateMemStorage(0);
	CvSURFParams params = cvSURFParams(500, 1);

	cvExtractSURF(gray, NULL, &keypoints, &descriptors, storage, params, 0);
	return keypoints;
}

CvSeq* detectFeaturesWithStar(IplImage *imagem)
{
	int i = 0;
	int maxSize = 8;
	int responseThreshold = 6;
	int lineThresholdProjected = 6;
	int lineThresholdBinarized = 6;
	int suppressNonmaxSize = 0;

	CvStarDetectorParams star_param =
		cvStarDetectorParams(
		maxSize,
		responseThreshold,
		lineThresholdProjected,
		lineThresholdBinarized,
		suppressNonmaxSize
	);

	IplImage *gray = cvCreateImage(cvSize(imagem->width, imagem->height), imagem->depth, 1);
	cvCvtColor(imagem, gray, CV_BGR2GRAY);

	CvMemStorage *storage = cvCreateMemStorage(0);
	CvSeq *keypoints = cvGetStarKeypoints(gray, storage, star_param);

	return keypoints;
}

vector<KeyPoint> detectFeaturesWithFast(IplImage *imagem)
{
	IplImage *gray = cvCreateImage(cvSize(imagem->width, imagem->height), imagem->depth, 1);
	cvCvtColor(imagem, gray, CV_BGR2GRAY);

	Mat mat(gray);
	vector<KeyPoint> keypoints2;

	FAST(mat, keypoints2, 9, true);

	return keypoints2;
}

void drawFeature(IplImage* image, CvPoint pt, int size, CvScalar color)
{
	CvPoint center;
	int radius;

	center.x = cvRound(pt.x);
	center.y = cvRound(pt.y);
	radius = cvRound(size * 1.2/9. * 2);

	cvCircle(image, center, radius, color, 1, 8, 0);
}

void drawSURFFeatures(IplImage *imagem, CvSeq *keypoints)
{
	int i;

	for(i = 0; i < keypoints->total; i++)
	{
		CvSURFPoint *r = (CvSURFPoint*) cvGetSeqElem(keypoints, i);
		drawFeature(imagem, cvPoint(r-> pt.x, r-> pt.y), r->size, cvScalar(255, 0, 0, 0));
	}
}

void drawStarFeatures(IplImage *imagem, CvSeq *keypoints)
{
	int i;

	for(i = 0; i < keypoints->total; i++)
	{
		CvStarKeypoint *r = (CvStarKeypoint*) cvGetSeqElem(keypoints, i);
		drawFeature(imagem, r->pt, r->size, cvScalar(0, 255, 0, 0));
	}
}

void drawFASTFeatures(IplImage *imagem, vector<KeyPoint> &keypoints)
{
	int i;

	for(i = 0; i < keypoints.size(); i++)
	{
		KeyPoint k;

		drawFeature(
			imagem,
			cvPoint(keypoints[i].pt.x, keypoints[i].pt.y),
			keypoints[i].size,
			cvScalar(0, 0, 255, 0)
		);
	}
}

int main(int argc, char* argv[])
{
	CvCapture* capture = cvCaptureFromCAM( CV_CAP_ANY );

	if (!capture)
	{
		printf("ERROR: capture is NULL\n");
		return -1;
	}

	cvNamedWindow("SURF", CV_WINDOW_AUTOSIZE);
	cvNamedWindow("STAR", CV_WINDOW_AUTOSIZE);
	cvNamedWindow("FAST", CV_WINDOW_AUTOSIZE);

	while (1)
	{
		IplImage *frame_big = cvQueryFrame(capture);
		IplImage *frame = cvCreateImage (cvSize(256, 256), frame_big->depth, frame_big->nChannels);

		cvResize(frame_big, frame, CV_INTER_CUBIC);

		CvSeq *SURF_keypoints = detectFeaturesWithSurf(frame);
		CvSeq *Star_keypoints = detectFeaturesWithStar(frame);
		vector<KeyPoint> Fast_keypoints = detectFeaturesWithFast(frame);

		printf("SURF: %d STAR: %d FAST: %d\n", SURF_keypoints->total, Star_keypoints->total, Fast_keypoints.size());

		IplImage *surf_keypoints_img = cvCreateImage(cvSize(frame->width, frame->height), frame->depth, 3);
		IplImage *star_keypoints_img = cvCreateImage(cvSize(frame->width, frame->height), frame->depth, 3);
		IplImage *fast_keypoints_img = cvCreateImage(cvSize(frame->width, frame->height), frame->depth, 3);

		cvCopyImage(frame, surf_keypoints_img);
		cvCopyImage(frame, star_keypoints_img);
		cvCopyImage(frame, fast_keypoints_img);

		drawSURFFeatures(surf_keypoints_img, SURF_keypoints);
		drawStarFeatures(star_keypoints_img, Star_keypoints);
		drawFASTFeatures(fast_keypoints_img, Fast_keypoints);

		cvShowImage("SURF", surf_keypoints_img);
		cvShowImage("STAR", star_keypoints_img);
		cvShowImage("FAST", fast_keypoints_img);

		cvReleaseImage(&surf_keypoints_img);
		cvReleaseImage(&star_keypoints_img);
		cvReleaseImage(&fast_keypoints_img);

		if ((cvWaitKey(10) & 255) == 27)
			break;

		cvReleaseImage(&frame);
	}

	cvReleaseCapture(&capture);
	cvDestroyWindow("SURF");
	cvDestroyWindow("STAR");
	cvDestroyWindow("FAST");

	return 0;
}

